#ifndef matrix44_h__
#define matrix44_h__
#include <memory.h>
#include "coremath.h"
#include "vector.h"
#include "quaternion.h"
#include "util/assert.h"

namespace gecko
{

// Matrix44 - 4 x 4 matrix
//
//  0   1   2   3
//  4   5   6   7
//  8   9   10  11
//  12  13  14  15
//
//////////////////////////////////////////////////////////////////////////
class Matrix44
{
public:
	float m[16];

	Matrix44();
	Matrix44(const Quaternion& q);

	void Zero();
	void Identity();
	static Matrix44 MakeIdentity();
	static Matrix44 MakeZero();
	Matrix44 Inverse() const;
	Matrix44 Transpose() const;
	void SetupScale(float sx, float sy, float sz);
	void SetupRotationX(float angle);
	void SetupRotationY(float angle);
	void SetupRotationZ(float angle);
	void SetupTranslation(float x, float y, float z);
	template < typename T > void SetupTranslation(const VectorT<T> &v);
	template < typename T > void SetupScale(const VectorT<T> &s);
	template < typename T > void SetupRotation(const VectorT<T> &vDir, float angle);
	template < typename T > void SetPosition(const VectorT<T>& vPosition);

	void RemoveRotationX();
	void RemoveRotationY();
	void RemoveRotationZ();

	template < typename T > VectorT<T> GetEulerAngles() const;
	template < typename T > VectorT<T> GetAbsEulerAngles() const;
	template < typename T > void GetPosition(VectorT<T> & position) const;

	void operator =(const Matrix44& rhs);

	static const Matrix44 ZERO;
	static const Matrix44 IDENTITY;
};


inline Matrix44::Matrix44()
{
	Identity();
}

inline Matrix44::Matrix44(const Quaternion& q)
{
	m[0] = 1.0f - (2.0f * q.y * q.y) - (2.0f * q.z * q.z);
	m[1] = (2.0f * q.x * q.y) + (2.0f * q.w * q.z);
	m[2] = (2.0f * q.x * q.z) - (2.0f * q.w * q.y);
	m[3] = 0.0f;
	m[4] = (2.0f * q.x * q.y) - (2.0f * q.w * q.z);
	m[5] = 1.0f - (2.0f * q.x * q.x) - (2.0f * q.z * q.z);
	m[6] = (2.0f * q.y * q.z) + (2.0f * q.w * q.x);
	m[7] = 0.0f;
	m[8] = (2.0f * q.x * q.z) + (2.0f * q.w * q.y);
	m[9] = (2.0f * q.y * q.z) - (2.0f * q.w * q.x);
	m[10] = 1.0f - (2.0f * q.x * q.x) - (2.0f * q.y * q.y);
	m[11] = 0.0f;
	m[12] = 0.0f;
	m[13] = 0.0f;
	m[14] = 0.0f;
	m[15] = 1.0f;
}

inline void Matrix44::Zero() 
{ 
	memset(m, 0, sizeof(float[16])); 
}

inline void Matrix44::Identity()
{
	memset(m, 0, sizeof(float[16]));
	m[0] = m[5] = m[10] = m[15] = 1;
}

inline Matrix44 Matrix44::MakeZero()
{
	Matrix44 mat;
	mat.Zero();
	return mat;
}

inline Matrix44 Matrix44::MakeIdentity()
{
	Matrix44 mat;
	mat.Identity();
	return mat;
}

inline void Matrix44::SetupRotationX(float angle)
{
	Identity();
	float fCos = cosf(angle);
	float fSin = sinf(angle);

	m[5] = fCos;
	m[6] = fSin;
	m[9] = -fSin;
	m[10] = fCos;
}

inline void Matrix44::SetupRotationY(float angle)
{
	Identity();
	float fCos = cosf(angle);
	float fSin = sinf(angle);

	m[0] = fCos;
	m[2] = -fSin;
	m[8] = fSin;
	m[10] = fCos;
}

inline void Matrix44::SetupRotationZ(float angle)
{
	Identity();
	float fCos = cosf(angle);
	float fSin = sinf(angle);

	m[0] = fCos;
	m[1] = fSin;
	m[4] = -fSin;
	m[5] = fCos;
}

inline void Matrix44::SetupTranslation(float x, float y, float z)
{
	Identity();
	m[12] = x;
	m[13] = y;
	m[14] = z;
}

template < typename T >
inline void Matrix44::SetupTranslation(const VectorT<T> &v)
{
	Identity();
	m[12] = v.x;
	m[13] = v.y;
	m[14] = v.z;
}

inline void Matrix44::SetupScale(float sx, float sy, float sz)
{
	Identity();

	m[0] = sx;
	m[5] = sy;
	m[10] = sz;
}

template < typename T >
inline void Matrix44::SetupScale(const VectorT<T> &s)
{
	Identity();

	m[0] = s.x;
	m[5] = s.y;
	m[10] = s.z;
}

template < typename T >
inline void Matrix44::SetupRotation(const VectorT<T> &vDir, float angle)
{
	//gtAssert(vDir.magnitude() == 1.0f || vDir.magnitude() == 0.0f, "Dir vector must be normalised");

	float fCos = cosf(angle);
	float fSin = sinf(angle);

	m[0] =  ( vDir.x * vDir.x ) * ( 1.0f - fCos ) + fCos;
	m[4] =  ( vDir.x * vDir.y ) * ( 1.0f - fCos ) - (vDir.z * fSin);
	m[8] =  ( vDir.x * vDir.z ) * ( 1.0f - fCos ) + (vDir.y * fSin);

	m[1] =  ( vDir.y * vDir.x ) * ( 1.0f - fCos ) + (vDir.z * fSin);
	m[5] =  ( vDir.y * vDir.y ) * ( 1.0f - fCos ) + fCos ;
	m[9] =  ( vDir.y * vDir.z ) * ( 1.0f - fCos ) - (vDir.x * fSin);

	m[2] =  ( vDir.z * vDir.x ) * ( 1.0f - fCos ) - (vDir.y * fSin);
	m[6] =  ( vDir.z * vDir.y ) * ( 1.0f - fCos ) + (vDir.x * fSin);
	m[10] = ( vDir.z * vDir.z ) * ( 1.0f - fCos ) + fCos;

	m[12] = m[13] = m[14] = 0.0f;
	m[3] = m[7] = m[11] = 0.0f;
	m[15] = 1.0f;
}

template < typename T >
inline void Matrix44::SetPosition(const VectorT<T>& vPosition)
{
	m[12] = vPosition.x;
	m[13] = vPosition.y;
	m[14] = vPosition.z;
}

template < typename T >
inline VectorT<T> operator *(const Matrix44 &mat, const VectorT<T> &vSrc)
{
	VectorT<T> vDest;

	T x = vSrc.x * mat.m[0] + vSrc.y * mat.m[4] + vSrc.z * mat.m[8] + mat.m[12];
	T y = vSrc.x * mat.m[1] + vSrc.y * mat.m[5] + vSrc.z * mat.m[9] + mat.m[13];
	T z = vSrc.x * mat.m[2] + vSrc.y * mat.m[6] + vSrc.z * mat.m[10] + mat.m[14];
	T w = vSrc.x * mat.m[3] + vSrc.y * mat.m[7] + vSrc.z * mat.m[11] + mat.m[15];

	//if( fabs( w ) < g_EPSILON )
	//    return E_INVALIDARG;

	vDest.x = x/w;
	vDest.y = y/w;
	vDest.z = z/w;

	return vDest;
}

template < typename T >
inline VectorT<T> operator *(const VectorT<T> &vSrc, const Matrix44 &mat)
{
	VectorT<T> vDest;

	/*T x = vSrc.x * mat.m[0] + vSrc.y * mat.m[4] + vSrc.z * mat.m[8] + mat.m[12];
	T y = vSrc.x * mat.m[1] + vSrc.y * mat.m[5] + vSrc.z * mat.m[9] + mat.m[13];
	T z = vSrc.x * mat.m[2] + vSrc.y * mat.m[6] + vSrc.z * mat.m[10] + mat.m[14];
	T w = vSrc.x * mat.m[3] + vSrc.y * mat.m[7] + vSrc.z * mat.m[11] + mat.m[15];*/

	vDest.x = vSrc.x * mat.m[0] + vSrc.y * mat.m[4] + vSrc.z * mat.m[8] + mat.m[12];
	vDest.y = vSrc.x * mat.m[1] + vSrc.y * mat.m[5] + vSrc.z * mat.m[9] + mat.m[13];
	vDest.z = vSrc.x * mat.m[2] + vSrc.y * mat.m[6] + vSrc.z * mat.m[10] + mat.m[14];
	
#if defined(_DEBUG)
	float w = vSrc.x * mat.m[3] + vSrc.y * mat.m[7] + vSrc.z * mat.m[11] + mat.m[15];
	Assert(w == 1.0f, "w should always be 1");
#endif

	//if( fabs( w ) < g_EPSILON )
	//    return E_INVALIDARG;

	/*float invW = 1 / w;

	vDest.x = x * invW;
	vDest.y = y * invW;
	vDest.z = z * invW;*/

	return vDest;
}

template < typename T >
inline void VectorMultiply(VectorT<T>& vOut, const VectorT<T> &vIn, const Matrix44 &matIn)
{
	vOut.x = vIn.x * matIn.m[0] + vIn.y * matIn.m[4] + vIn.z * matIn.m[8] + matIn.m[12];
	vOut.y = vIn.x * matIn.m[1] + vIn.y * matIn.m[5] + vIn.z * matIn.m[9] + matIn.m[13];
	vOut.z = vIn.x * matIn.m[2] + vIn.y * matIn.m[6] + vIn.z * matIn.m[10] + matIn.m[14];
	
#if defined(_DEBUG)
	float w = vIn.x * matIn.m[3] + vIn.y * matIn.m[7] + vIn.z * matIn.m[11] + matIn.m[15];
	Assert(w == 1.0f, "w should always be 1");
#endif
}

inline Matrix44 operator *(const Matrix44 &b, const Matrix44 &a)
{
	Matrix44 matResult;
	matResult.m[0] = a.m[0] * b.m[0] +
		a.m[4] * b.m[1] +
		a.m[8] * b.m[2] +
		a.m[12] * b.m[3];
	matResult.m[4] = a.m[0] * b.m[4] +
		a.m[4] * b.m[5] +
		a.m[8] * b.m[6] +
		a.m[12] * b.m[7];
	matResult.m[8] = a.m[0] * b.m[8] +
		a.m[4] * b.m[9] +
		a.m[8] * b.m[10] +
		a.m[12] * b.m[11];
	matResult.m[12] = a.m[0] * b.m[12] +
		a.m[4] * b.m[13] +
		a.m[8] * b.m[14] +
		a.m[12] * b.m[15];
	matResult.m[1] = a.m[1] * b.m[0] +
		a.m[5] * b.m[1] +
		a.m[9] * b.m[2] +
		a.m[13] * b.m[3];
	matResult.m[5] = a.m[1] * b.m[4] +
		a.m[5] * b.m[5] +
		a.m[9] * b.m[6] +
		a.m[13] * b.m[7];
	matResult.m[9] = a.m[1] * b.m[8] +
		a.m[5] * b.m[9] +
		a.m[9] * b.m[10] +
		a.m[13] * b.m[11];
	matResult.m[13] = a.m[1] * b.m[12] +
		a.m[5] * b.m[13] +
		a.m[9] * b.m[14] +
		a.m[13] * b.m[15];
	matResult.m[2] = a.m[2] * b.m[0] +
		a.m[6] * b.m[1] +
		a.m[10] * b.m[2] +
		a.m[14] * b.m[3];
	matResult.m[6] = a.m[2] * b.m[4] +
		a.m[6] * b.m[5] +
		a.m[10] * b.m[6] +
		a.m[14] * b.m[7];
	matResult.m[10] = a.m[2] * b.m[8] +
		a.m[6] * b.m[9] +
		a.m[10] * b.m[10] +
		a.m[14] * b.m[11];
	matResult.m[14] = a.m[2] * b.m[12] +
		a.m[6] * b.m[13] +
		a.m[10] * b.m[14] +
		a.m[14] * b.m[15];
	matResult.m[3] = a.m[3] * b.m[0] +
		a.m[7] * b.m[1] +
		a.m[11] * b.m[2] +
		a.m[15] * b.m[3];
	matResult.m[7] = a.m[3] * b.m[4] +
		a.m[7] * b.m[5] +
		a.m[11] * b.m[6] +
		a.m[15] * b.m[7];
	matResult.m[11] = a.m[3] * b.m[8] +
		a.m[7] * b.m[9] +
		a.m[11] * b.m[10] +
		a.m[15] * b.m[11];
	matResult.m[15] = a.m[3] * b.m[12] +
		a.m[7] * b.m[13] +
		a.m[11] * b.m[14] +
		a.m[15] * b.m[15];
	return matResult;
}

inline Matrix44 Matrix44::Transpose() const
{
	Matrix44 matResult;
	matResult.m[0] = m[0];
	matResult.m[1] = m[4];
	matResult.m[2] = m[8];
	matResult.m[3] = m[12];
	matResult.m[4] = m[1];
	matResult.m[5] = m[5];
	matResult.m[6] = m[9];
	matResult.m[7] = m[13];
	matResult.m[8] = m[2];
	matResult.m[9] = m[6];
	matResult.m[10] = m[10];
	matResult.m[11] = m[14];
	matResult.m[12] = m[3];
	matResult.m[13] = m[7];
	matResult.m[14] = m[11];
	matResult.m[15] = m[15];
	return matResult;
}

inline Matrix44 Matrix44::Inverse() const
{
	float fA0 = m[ 0] * m[ 5] - m[ 1] * m[ 4];
	float fA1 = m[ 0] * m[ 6] - m[ 2] * m[ 4];
	float fA2 = m[ 0] * m[ 7] - m[ 3] * m[ 4];
	float fA3 = m[ 1] * m[ 6] - m[ 2] * m[ 5];
	float fA4 = m[ 1] * m[ 7] - m[ 3] * m[ 5];
	float fA5 = m[ 2] * m[ 7] - m[ 3] * m[ 6];
	float fB0 = m[ 8] * m[13] - m[ 9] * m[12];
	float fB1 = m[ 8] * m[14] - m[10] * m[12];
	float fB2 = m[ 8] * m[15] - m[11] * m[12];
	float fB3 = m[ 9] * m[14] - m[10] * m[13];
	float fB4 = m[ 9] * m[15] - m[11] * m[13];
	float fB5 = m[10] * m[15] - m[11] * m[14];

	float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
	if (fabs(fDet) <= ZERO_TOLERANCE)
	{
		return Matrix44::ZERO;
	}

	Matrix44 kInv;
	kInv.m[ 0] =
		+ m[ 5]*fB5 - m[ 6]*fB4 + m[ 7]*fB3;
	kInv.m[ 4] =
		- m[ 4]*fB5 + m[ 6]*fB2 - m[ 7]*fB1;
	kInv.m[ 8] =
		+ m[ 4]*fB4 - m[ 5]*fB2 + m[ 7]*fB0;
	kInv.m[12] =
		- m[ 4]*fB3 + m[ 5]*fB1 - m[ 6]*fB0;
	kInv.m[ 1] =
		- m[ 1]*fB5 + m[ 2]*fB4 - m[ 3]*fB3;
	kInv.m[ 5] =
		+ m[ 0]*fB5 - m[ 2]*fB2 + m[ 3]*fB1;
	kInv.m[ 9] =
		- m[ 0]*fB4 + m[ 1]*fB2 - m[ 3]*fB0;
	kInv.m[13] =
		+ m[ 0]*fB3 - m[ 1]*fB1 + m[ 2]*fB0;
	kInv.m[ 2] =
		+ m[13]*fA5 - m[14]*fA4 + m[15]*fA3;
	kInv.m[ 6] =
		- m[12]*fA5 + m[14]*fA2 - m[15]*fA1;
	kInv.m[10] =
		+ m[12]*fA4 - m[13]*fA2 + m[15]*fA0;
	kInv.m[14] =
		- m[12]*fA3 + m[13]*fA1 - m[14]*fA0;
	kInv.m[ 3] =
		- m[ 9]*fA5 + m[10]*fA4 - m[11]*fA3;
	kInv.m[ 7] =
		+ m[ 8]*fA5 - m[10]*fA2 + m[11]*fA1;
	kInv.m[11] =
		- m[ 8]*fA4 + m[ 9]*fA2 - m[11]*fA0;
	kInv.m[15] =
		+ m[ 8]*fA3 - m[ 9]*fA1 + m[10]*fA0;

	float fInvDet = 1.0f/fDet;
	kInv.m[ 0] *= fInvDet;
	kInv.m[ 1] *= fInvDet;
	kInv.m[ 2] *= fInvDet;
	kInv.m[ 3] *= fInvDet;
	kInv.m[ 4] *= fInvDet;
	kInv.m[ 5] *= fInvDet;
	kInv.m[ 6] *= fInvDet;
	kInv.m[ 7] *= fInvDet;
	kInv.m[ 8] *= fInvDet;
	kInv.m[ 9] *= fInvDet;
	kInv.m[10] *= fInvDet;
	kInv.m[11] *= fInvDet;
	kInv.m[12] *= fInvDet;
	kInv.m[13] *= fInvDet;
	kInv.m[14] *= fInvDet;
	kInv.m[15] *= fInvDet;

	return kInv;
}

template < typename T >
inline VectorT<T> Matrix44::GetEulerAngles() const
{
	VectorT<T> vAngles;

	vAngles.x = -asinf(m[6]);
	vAngles.y = atan2f(m[2], m[10]);
	vAngles.z = atan2f(m[4],m[5]);

	return vAngles;
}

template < typename T >
inline VectorT<T> Matrix44::GetAbsEulerAngles() const
{
	VectorT<T> vAngles;

	vAngles.x = -asinf(m[6]);
	vAngles.y = atan2f(m[2], m[10]);


	vAngles.z = atan2f(m[4],m[5]);

	if ( vAngles.z < 0.0f )
	{
		vAngles.z = RADIANS_360 + vAngles.z;
	}

	return vAngles;
}

template < typename T >
inline void Matrix44::GetPosition(VectorT<T> & position) const
{
	position.x = m[12];
	position.y = m[13];
	position.z = m[14];
}

inline void Matrix44::RemoveRotationX()
{
	m[0] = 1.0f;
	m[1] = 0.0f;
	m[2] = 0.0f;
	m[4] = 0.0f;
}

inline void Matrix44::RemoveRotationY()
{
	m[4] = 0.0f;
	m[5] = 1.0f;
	m[6] = 0.0f;
	m[7] = 0.0f;
}

inline void Matrix44::RemoveRotationZ()
{
	m[8] =  0.0f;
	m[9] =  0.0f;
	m[10] = 1.0f;
	m[11] = 0.0f;
}

inline void Matrix44::operator =(const Matrix44 &rhs)
{
	memcpy(m, rhs.m, sizeof(float) * 16);
}

}

#endif // matrix44_h__

